# -*- coding:utf-8 -*-
"""
Created on Apr 27, 2011

@author: Lithuanian
"""
from random import randint
from multi_robot_problem import MultiRobotState
class Tester:
    DEBUG = False
    
    #obstacle types
    OBSTACLES_RAND      = 1
    OBSTACLES_WALLS     = 2
    OBSTACLES_POCKETS   = 3
    OBSTACLES_ALL       = 4
    
    HORIZONTAL  = 1
    VERTICAL    = 2 
    
    #maximal attempts at generating a problem space
    MAX_FAIL_ATTEMPTS = 20
    #maxinal attempts at a single obstacle
    MAX_SINGLE_OBSTACLE_FAIL = 5
    
    #pocket open side
    OPEN_LEFT   = 1
    OPEN_RIGHT  = 2
    OPEN_UP     = 3
    OPEN_DOWN   = 4
    
    def __init__(self, minWidth, maxWidth, minHeight, maxHeight, maxRobots, maxPiles, maxObstacles, \
                 obstacleType, wallsLenght, wallsVariance, pocketWidth, \
                 pocketWidthVariance, pocketHeight, pocketHeightVariance, minPockets = 0, minWalls = 0):
        '''
        @param minWidth 
        @param maxWidth
        @param minHeight 
        @param maxHeight
        @param maxRobots: Maximal number of robots in the generated problem space
        @param maxPiles: Maximal number of piles in the generated problem space
        @param maxObstacles: Maximal number of obstacles in the generated problem space
        @param obstacleType: random, walls, or pockets
        @param wallsLenght: lenght of wall obstacles
        @param wallsVariance: the variance in wall lenght 
        @param pocketWidth: width of pocket obstacles
        @param pocketWidthVariance: the cariance in wall width 
        @param pocketHeight: height of pocket obstacles
        @param pocketHeightVariance: the variance in wall height 
        @param minPockets: minmal number of pockets if obstacleType is OBSTACLE_ALL
        @param minWalls: minmal number of walls if obstacleType is OBSTACLE_ALL 
        '''
        self.maxWidth               = maxWidth
        self.maxHeight              = maxHeight
        self.maxRobots              = maxRobots
        self.maxPiles               = maxPiles
        self.maxObstacles           = maxObstacles
        self.obstacleType           = obstacleType
        self.wallsLenght            = wallsLenght
        self.wallsVariance          = wallsVariance
        self.pocketWidth            = pocketWidth
        self.pocketWidthVariance    = pocketWidthVariance
        self.pocketHeight           = pocketHeight
        self.pocketHeightVariance   = pocketHeightVariance
        self.minWidth               = minWidth
        self.minHeight              = minHeight
        self.minPockets             = minPockets
        self.minWalls               = minWalls
    
    def getRandomPoint(self, minX, maxX, minY, maxY):
        x = randint(minX, maxX)
        y = randint(minY, maxY)
        return (x,y)
    
    def generateProblemSpace(self):
        '''
        Generate random problem space as defined in the MultiRobotState class with
        with the parameters given in the C'tor
        '''
        fails = 0
        width       = randint(self.minWidth, self.maxWidth)
        height      = randint(self.minHeight, self.maxHeight)
        
        #generate robots
        robots = set()
        i = 0
        while i < self.maxRobots:
            point = self.getRandomPoint(0, width-1, 0, height-1) 
            
            #check if the robot already in the set
            if point in robots:
                continue
            #add robot to the set
            robots.add(point)
            i += 1
        
        #generate piles
        i = 0
        piles = set()
        while i < self.maxPiles:
            point = self.getRandomPoint(0, width-1, 0, height-1)
            #check if the pile already in the set or is robot
            if point in piles or point in robots:
                continue
            #add pile to the set
            piles.add(point)
            i += 1
        
        #generate obstacles
        i = 0
        numOfWalls = 0
        numOfPockets = 0
        obstacles = set()
        while i < self.maxObstacles:
            #to avoid infinite loop, if MAX_FAIL_ATTEMPTS reached - return a failure 
            if fails > self.MAX_FAIL_ATTEMPTS:
                return None
            
            #if all kinds of obstacles are welcome
            if self.obstacleType == self.OBSTACLES_ALL:
                #add minimal number of walls and pockets when obstacleType is OBSTACLE_ALL
                while numOfWalls < self.minWalls:
                    if fails > self.MAX_FAIL_ATTEMPTS:
                        return None
                    point = self.getRandomPoint(0, width-1, 0, height-1)
                    wall = self.makeWall(point[0], point[1], width, height, piles, robots, obstacles)
                    if wall is None:
                        fails += 1
                        continue
                    i += 1
                    numOfWalls += 1
                    obstacles = obstacles.union(wall)
                
                while numOfPockets < self.minPockets:
                    point = self.getRandomPoint(0, width-1, 0, height-1)
                    pocket = self.makePocket(point[0], point[1], width, height, piles, robots, obstacles)
                    if pocket is None:
                        fails += 1
                        continue
                    i += 1
                    numOfPockets += 1
                    obstacles = obstacles.union(pocket)
                
                #there are minimal number of walls and pockets - now generate random obstecales
                obstacle_all_choise = randint(1,3)
            else:
                obstacle_all_choise = -1
            
            point = self.getRandomPoint(0, width-1, 0, height-1)
            #check if the obstacle already in the set or is robot or pile
            if point in piles or point in robots or point in obstacles:
                fails += 1
                continue
            
            if self.obstacleType == self.OBSTACLES_RAND or obstacle_all_choise == self.OBSTACLES_RAND:
                #add random one area obstacle
                obstacles.add(point)
                i += 1
                continue
            
            if self.obstacleType == self.OBSTACLES_WALLS or obstacle_all_choise == self.OBSTACLES_WALLS:
                #add random wall with root point (x,y)
                wall = self.makeWall(point[0], point[1], width, height, piles, robots, obstacles)
                if wall is None:
                    fails += 1
                    continue
                obstacles = obstacles.union(wall)
                i += 1
                continue
            
            if self.obstacleType == self.OBSTACLES_POCKETS or obstacle_all_choise == self.OBSTACLES_POCKETS:
                #add random pocket with root point (x,y)
                pocket = self.makePocket(point[0], point[1], width, height, piles, robots, obstacles)
                if pocket is None:
                    fails += 1
                    continue
                obstacles = obstacles.union(pocket) 
                i += 1
                continue
        
        ######DEBUG#####
        if self.DEBUG:
            print "height: " + str(height) + ' , width: ' + str(width)
            print "------Robots------"
            print robots
            print "------Piles------"
            print piles
            print "------Obstacles------"
            print obstacles
        ################    
        #create problem state
        problem =  MultiRobotState(width, height, tuple(list(robots)), frozenset(piles), frozenset(obstacles))
        if self.isSolvableProblem(problem):
            return problem
        return None
    
    def nextObstaclePoint(self, rootX, rootY, direction):
        '''
        @param rootX:
        @param rootY:
        @param direction:   
        '''
        if direction is self.OPEN_DOWN:
            return (rootX, rootY + 1)
        if direction is self.OPEN_LEFT:
            return (rootX - 1, rootY)
        if direction is self.OPEN_RIGHT:
            return (rootX + 1, rootY)
        if direction is self.OPEN_UP:
            return (rootX, rootY - 1)
    
    def checkWalls(self, rootX, rootY, lenght, direction, robots, piles, \
                   obstacles, width, height, wall):
        '''
        check the wall is a legal one
        @return: None on falure and last point added on success
        '''
        point = (rootX,rootY)
        if point in robots or point in piles or point in obstacles:
            return None
        wall.add(point)
        for i in xrange(0, lenght):
            point = self.nextObstaclePoint(point[0], point[1], direction)
            if point in robots or point in piles \
                or point in obstacles or point[0] >= width or point[1] >= height \
                or point[0] < 0 or point[1] < 0:
                return None
            wall.add(point)
        
        return point
                 
    def makeWall(self, rootX, rootY, width, height, piles, robots, obstacles):
        '''
        make a wall obstacle according to the specified parameters and object creation params
        @param rootX: horizontal value of the wall start point
        @param rootY: vertical value of the wall start point
        @param width: board width
        @param height: board height 
        @param piles
        @param robots
        @param obstacles
        @return: set of points - the wall location    
        '''
        fail = 0
        while fail < self.MAX_SINGLE_OBSTACLE_FAIL:
            isFail = False
            #calculate wall lenght
             
            lenght = randint(self.wallsLenght - self.wallsVariance, \
                           self.wallsLenght + self.wallsVariance)
            
            direction = randint(self.HORIZONTAL, self.VERTICAL)
            
            wall = set()
            wall.add((rootX, rootY))
            #check that wall is legal and build it on the way
            for i in xrange(1, lenght):
                if direction is self.HORIZONTAL:
                    if (rootX+i,rootY) in robots or (rootX+i,rootY) in piles \
                        or (rootX+i,rootY) in obstacles or rootX+i >= width:
                        fail += 1
                        isFail = True
                        break
                    wall.add((rootX+i, rootY))
                else:
                    if (rootX,rootY+i) in robots or (rootX,rootY+i) in piles \
                        or (rootX,rootY+i) in obstacles or rootY+i >= height:
                        fail += 1
                        isFail = True
                        break
                    wall.add((rootX, rootY+i))
            #wall created successfuly - break, and return it
            if not isFail:
                break
        #failed to create a legal wall - return None
        if isFail:
            return None
        #wall is legal, return wall points as a set
        return wall
            
        
    def makePocket(self, rootX , rootY, width, height, piles, robots, obstacles):
        '''
        create a single random pocket
        @return: set of points describing the pocket
        '''
        openSide = randint(self.OPEN_LEFT, self.OPEN_DOWN)
        if openSide is self.OPEN_DOWN or openSide is self.OPEN_UP:
            pocket = self.makeVerticalPocket(rootX, rootY, openSide, height, width, piles, robots, obstacles)
        else:
            pocket = self.makeHorizontalPocket(rootX, rootY, openSide, height, width, piles, robots, obstacles)
        if pocket is None:
            return None
        return pocket
    
    def makeVerticalPocket(self, rootX, rootY, openSide, height, width, piles, robots, obstacles):
        '''
        create a pocket that it's open side is horizontal (which makes the pocket vertical)
        @return: set of points describing the pocket
        '''
        fail = 0
        while fail < self.MAX_SINGLE_OBSTACLE_FAIL:
            walls = set()
            #calculate walls lenght
            #vertical walls
            farPocketWall   = randint(self.pocketHeight - self.pocketWidthVariance,\
                                           self.pocketHeight + self.pocketWidthVariance)
            closePocketWall = randint(self.pocketHeight - self.pocketHeightVariance, \
                                          self.pocketHeight + self.pocketHeightVariance)
                
            #horizontal wall
            horizontalWall  = randint(self.pocketWidth - self.pocketWidthVariance,\
                                          self.pocketWidth + self.pocketWidthVariance)
            if openSide is self.OPEN_DOWN:
                #opent side is DOWN advance upwards
                
                #check that walls are legal and build it on the way
                point = self.checkWalls(rootX, rootY, closePocketWall, self.OPEN_UP, \
                                        robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                point = self.checkWalls(point[0], point[1], horizontalWall, self.OPEN_RIGHT,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                
                point = self.checkWalls(point[0], point[1], farPocketWall, self.OPEN_DOWN,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                break
            else:
                #opent side is DOWN advance downwards
               
                #check that walls are legal and build it on the way
                point = self.checkWalls(rootX, rootY, closePocketWall, self.OPEN_DOWN, \
                                        robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                point = self.checkWalls(point[0], point[1], horizontalWall, self.OPEN_RIGHT,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                
                point = self.checkWalls(point[0], point[1], farPocketWall, self.OPEN_UP,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue    
                break
        if fail is self.MAX_SINGLE_OBSTACLE_FAIL:
            return None
        return walls
    
    def makeHorizontalPocket(self, rootX, rootY, openSide, height, width, piles, robots, obstacles):
        '''
        create a pocket that it's open side is vertical(which makes the pocket horizontal)
        @return: set of points describing the pocket
        '''
        fail = 0
        while fail < self.MAX_SINGLE_OBSTACLE_FAIL:
            walls = set()
            #calculate walls lenght
            #vertical walls
            farPocketWall   = randint(self.pocketHeight - self.pocketWidthVariance,\
                                           self.pocketHeight + self.pocketWidthVariance)
            closePocketWall = randint(self.pocketHeight - self.pocketHeightVariance, \
                                          self.pocketHeight + self.pocketHeightVariance)
                
            #horizontal wall
            verticalWall = randint(self.pocketWidth - self.pocketWidthVariance,\
                                          self.pocketWidth + self.pocketWidthVariance)
            if openSide is self.OPEN_LEFT:
                #opent side is left advance RIGHT
                
                #check that walls are legal and build it on the way
                point = self.checkWalls(rootX, rootY, closePocketWall, self.OPEN_RIGHT, \
                                        robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                point = self.checkWalls(point[0], point[1], verticalWall, self.OPEN_DOWN,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                
                point = self.checkWalls(point[0], point[1], farPocketWall, self.OPEN_LEFT,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                break
            else:
                #opent side is right advance LEFT
               
                #check that walls are legal and build it on the way
                point = self.checkWalls(rootX, rootY, closePocketWall, self.OPEN_LEFT, \
                                        robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                point = self.checkWalls(point[0], point[1], verticalWall, self.OPEN_DOWN,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue
                
                point = self.checkWalls(point[0], point[1], farPocketWall, self.OPEN_RIGHT,\
                                         robots, piles, obstacles, width, height, walls)
                if point is None:
                    fail += 1
                    continue    
                break
        if fail is self.MAX_SINGLE_OBSTACLE_FAIL:
            return None
        return walls
    
    def printProblemSpaceParams(self, problem):
        print self.problemSpace2String(problem)
#        if problem is None:
#            print "Failed to generate a problemt..."
#        print "width = " + str(problem.width)
#        print "height = " + str(problem.height)
#        print "robots = " + str(problem.robots)
#        print "dirt_locations = " + str(problem.dirt_locations)
#        print "obstacle_locations = " + str(problem.obstacle_locations)
#        print "problem = MultiRobotState(width, height, robots, dirt_locations, obstacle_locations)"
        
    def problemSpace2String(self, problem):
        res = ""
        if problem is None:
            res = "Failed to generate a problemt..."
            return res
        res = "width = " + str(problem.width) + "\n"
        res+= "height = " + str(problem.height) + "\n"
        res+= "robots = " + str(problem.robots) + "\n"
        res+= "dirt_locations = " + str(problem.dirt_locations) + "\n"
        res+= "obstacle_locations = " + str(problem.obstacle_locations) + "\n"
        res+= "problem = MultiRobotState(width, height, robots, dirt_locations, obstacle_locations)" + "\n"
        return res
    
    def isSolvableProblem(self, problemState):
        '''
        Check if the problem is solvable
        '''
        reachable = set()
        closure = set()
        for r in problemState.robots:
            closure.add(r)
        while len(closure.difference(reachable)) > 0:
            reachable = closure.copy()
            for x,y in reachable:
                for dx, dy in [(1,0),(-1,0),(0,1),(0,-1)]:
                    xnew, ynew = x+dx, y+dy
                    if xnew < 0 or xnew >= problemState.width or ynew < 0 or ynew > problemState.height:
                        continue
                    if (xnew, ynew) in problemState.obstacle_locations:
                        continue
                    closure.add((xnew, ynew))
        
#        print len (closure)
        for p in problemState.dirt_locations:
            if not p in reachable:
                return False
        return True